/***************************************************************************
 *   Copyright (C) 2002-2011 by Ferdowsi University of Mashhad,            *
 *   Software Simulation and Modeling lab                                  *
 *   nexus@um.ac.ir                                                        *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <mutex>
#include <thread>
#include "../include/Trainer.h"
#include "../../Logger/include/Logger.h"
#include "../../Matematica/include/Matematica.h"


Trainer::Trainer(std::string log):limitRaio(0.3){

	log_end = log;

}


Trainer::Trainer(std::string host, std::string port){

	mConnection = new ConexaoTCP(host, port);//conexao comentada para teste de debug
	starInicial=true;


}

void Trainer::run()
{
    int contTemo;
	bool initOK = true;



   lacoTeste=0;

	//initParametros();//metodo para iniciar o jogo


	while(initOK)//inicio do loop onde iniciar a execução do treiner
 	{

		receive(); //recebendo mensagem do servidor


		if(initOK && mCont > 10)
		{


			if(mTipo == 1 || mTipo == 4)
			{
				std::cout<<"Entrei no experimento 1"<<std::endl;
				return;

				initExperimentoUm();
			}
			else if(mTipo == 2)

			{
				 std::cout<<"Entrei no experimento 2"<<std::endl;
				initExperimentoDois();

			}else if(mTipo == 3)
			{
		         std::cout<<"Entrei no experimento 3"<<std::endl;
		         return;
				initExperimentoTres();
			}

			initOK = false;
			//listaDadosAgentes();
		}
		//TODO: Inicio de chamadas de experimentos
		if(flagPog){

			ExperimentoUm();

			if(mTipo == 3)
			{
				ExperimentoTres();
			}



		}

		//TODO: Fim de chamadas de experimentos


       if(listEstJogo=="YES")

    	   listaDadosAgentes();//lista posicoes dos agente apos leitura de cada mensagem

		 if(getLastUpdateTime() >= mTempoMaximo  ){
			 aborteTeste();//game ove
 	      }
		if(agente3dV[0].getPlayMode()=="GameOver" ){

			initOK=false;
			estat.setMensagemsRecebidas(contMengCompl,contaMensParcial, contaMensCortada, mLastUpdateTime,sScoreLeft,sScoreRigth);
			estat.resulmoJogo(contAgente);
			estat.salvaDadosAgentes(contAgente);//Metodo que chamar funcao gravar dadosCoordenadas do agente

		}


   }

}

/*TODO:
 * InitParametros é chamado para inicializar as variaveis utilizadas nos experimentos, deve
 * ser chamado antes de entrar no loop principal do método run
 */
void
Trainer::initParametros()
{
		mDistanciaInicial = mDistancia;
		mContQuedas = 0;
		mTempo1 = 0;
		mTempo2 = 0;
		mSomaTempo = 0;
		mCont = 0;
		mCaido = false;
		mChegou = false;
		//Flags , fim1,fim2,fim3 para o experimento tres 3
		fim1 = true;
		fim2 = true;
		fim3 = true;

		setPlayMode("PlayOn");
		flagPog= false;
}

void
Trainer::initExperimentoUm()
{
	Coordenada3d beamBola(getAgentPosition().X()+mDistancia,getAgentPosition().Y(),0);
	Coordenada3d beamAgente(mPosicaoXAgente,0,0);
	Coordenada3d rotacaoAgente(0,0,0);
	moveBall(beamBola);
	moveAgent(10,LC_ESQUERDA,beamAgente,rotacaoAgente,0,0);
}

void
Trainer::initExperimentoDois()
{
	Coordenada3d beamBola(0,0,0);
	Coordenada3d beamAgente(mPosicaoXAgente,0,0);
	Coordenada3d rotacaoAgente(0,0,0);
	beamBola = posicaoBolaAngulo(mAnguloAgenteBola);//TODO: usar este método para o ExperimentoDois
	moveBall(beamBola);
	moveAgent(9,LC_ESQUERDA,beamAgente,rotacaoAgente,0,0);
}

void
Trainer::initExperimentoTres()
{
	Coordenada3d beamBola(getAgentPosition().X()+mDistancia,getAgentPosition().Y(),0);
	Coordenada3d beamAgente(mPosicaoXAgente,0,0);
	Coordenada3d rotacaoAgente(0,0,0);
	moveBall(beamBola);
	moveAgent(10,LC_ESQUERDA,beamAgente,rotacaoAgente,0,0);
}
void Trainer::send(std::string msg)
{
    unsigned int len = htonl(msg.size());
    std::string prefix((const char*)&len, sizeof(unsigned int));
    msg = prefix + msg;

    mConnection->enviarMensagem(msg);
}

void Trainer::receive()
{
    //update(mConnection->receberMensagem());
	 // updateNovo(mConnection->receberMensagem()); //novo update pegando todas as partes do agente
	 mensagemParseTreiner(mConnection->receberMensagem());//tratando mensatem em divisoes recebidas do servidor
	// mensagemParseTreiner( mensagem);//testando atualizacao como mensagem fixa
}

void Trainer::moveBall(Coordenada3d pos){
    std::stringstream ss;
    ss<<"(ball (pos "<<pos.X()<<" "<<pos.Y()<<" "<<pos.Z()<<"))";
    send(ss.str());
}

void Trainer::moveGround(Coordenada3d rot,Coordenada3d pos)
{
    std::stringstream ss;
    ss<<"(ground (rot "<<rot.X()<<" "<<rot.Y()<<" "<<rot.Z()<<")"<<" (pos "<<pos.X()<<" "<<pos.Y()<<" "<<pos.Z()<<"))";
    send(ss.str());
}
void Trainer::moveAgent(int unum,Lado side,Coordenada3d pos,Coordenada3d rot,double batteryLevel,double temperature)
{
    std::stringstream ss;
    std::string team=(side==LC_ESQUERDA)?"left":"right";
    /*ss<<"(agent (unum "<<unum<<") (team "<<team<<") (pos "<<pos.X()<<" "<<pos.Y()<<" "<<pos.Z()<<")"
    		"(move "<<rot.X()<<" "<<rot.Y()<<" "<<rot.Z()<<" "<<")"
    		"(battery "<<batteryLevel<<")"
    		"(temperature "<<temperature<<"))";*/
    //ss<<"(agent (pos "<<pos.X()<<" "<<pos.Y()<<" "<<0.0<<"))";

    ss<<"(agent (team "<<team<<")(unum "<<unum<<")(pos"<<pos.X()<<" "<<pos.Y()<<" "<<pos.Z()<<"))";

    send(ss.str());
}

void Trainer::setPlayMode(std::string playMode)
{
    send("(playMode " + playMode + ")");
}

double Trainer::getLastUpdateTime()
{
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);
	return mLastUpdateTime;
}

Matriz4d Trainer::getAgentTransform()
{
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);
	return mAgentTransform;
}

Coordenada3d Trainer::getHeadAgentPosition()
{
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);
	return mAgentTransformHead.getPosition();
}
Coordenada3d Trainer::getAgentPosition() {
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);
	//Coordenada3d translation(mAgentTransform.get(0,3),mAgentTransform.get(1,3),mAgentTransform.get(2,3));
	//return translation;
	return mAgentTransform.getPosition();
}

Matriz4d Trainer::getBallTransform()
{
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);
	return mBallTransform;
}




/*========================   Métodos novos =========================================*/
void Trainer::leitorArquivo(){/*Leitura de  logs*/

 std::string line;
 std::ifstream myfile (log_end); // ifstream = padrão ios:in

 if (myfile.is_open())
 {
   while (! myfile.eof() ) //enquanto end of file for false continua
   {

	 if(mLastUpdateTime >= mTempoMaximo){ break;}
	 //if(agente3dV[0].getPlayMode()=="GameOver"){ break;}
     getline (myfile,line); // como foi aberto em modo texto(padrão)
     mensagemParseTreiner(line);                      //e não binário(ios::bin) pega cada linha
     //std::cout <<"Linha do:"<< line << std::endl;
     if(listEstJogo=="YES")
       listaDadosAgentes();//listando agentes
   }
   myfile.close();

   estat.setMensagemsRecebidas(contMengCompl,contaMensParcial, contaMensCortada,mLastUpdateTime,sScoreLeft,sScoreRigth );
   estat.resulmoJogo(contAgente);
   estat.salvaDadosAgentes(contAgente);//Metodo que chamar funcao gravar dadosCoordenadas do agente

 }

 else std::cout << "Arquivo não encontrado\n";


}



void Trainer::setListaDadosJogo(std::string lisOp){

	listEstJogo = lisOp;

}

void Trainer::listaDadosAgentes(){

	 std::mutex mutex;
	 std::lock_guard<std::mutex> lock(mutex);
	 int i;
	 system("clear");

	  if(contAgente>12) lim=contAgente-11;else lim=contAgente;
	 //std::cout<<"\nLimite:"<<lim;
      std::cout<<"\nTime: "<<agente3dV[1].getTimeAgene(); std::cout<<"  Score_Left"<<sScoreLeft<<" Score_Rigth:"<<sScoreRigth<<" PlayMode: "<<sPlayMode;
     for(i=1;i<lim;i++){
    	 std::cout<<"\n+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n";
         std::cout<<"Agente"<<i;


          std::cout<<"\  NumeroAgente: "<<agente3dV[i].getNumero()<<" "<< agente3dV[i].getLado()<<"";//if(conAgR>1)
        	if(contAgente>11){ std::cout<<"\          NumeroAgente: "<<agente3dV[11+i].getNumero()<<" "<< agente3dV[11+i].getLado()<<std::endl;}else std::cout<<"\n";

    	  std::cout<<"Status: ";     if(agente3dV[i].getStatus()==true) std::cout<<"Agente Caido";   //if(conAgR>1)                                    //if(conAgR>1)
    		if(contAgente>11){	 std::cout<<"                      Status: "; if(agente3dV[11+i].getStatus()==true) std::cout<<"   Agente Caido";}


     }

     std::cout<<"\n+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n";
}


void Trainer::atualizarPosicoes(std::string subMsg,int numOb,double  time){

    bool graPosi = false;
	Matriz4d* transform=NULL; //um ponteiro para tipo Matriz4D
	ParserTrainer* SsubExpress = new ParserTrainer(subMsg); //uma estancia de Parse para percorrer mensagem



    if(numOb==0){


	      SsubExpress->in(2);//passa dois parenteses
	      if(SsubExpress->take()=="SLT"){//verificar se o proximo token e SLT

	    	  transform = readTransformationMatrix(SsubExpress);
	    	  			if (transform != NULL)
	    	  			{

	    	  			   agente3dV[numOb].setBoll(*transform);

	    	  			}
	      }


    }else{


    	    SsubExpress->in(3);

    	    agente3dV[numOb].setBoll(agente3dV[0].getBollM4());//setando valores da bola para os agentes
    	    agente3dV[numOb].setPlayMode(agente3dV[0].getPlayMode());

    	   if(SsubExpress->take()=="SLT")
    	   {
				transform = readTransformationMatrix(SsubExpress);
				if (transform != NULL)
				{
					agente3dV[numOb].setBody(*transform);

					if(posseDeBola(numOb)==1)agente3dV[numOb].setPosseBola(true); else agente3dV[numOb].setPosseBola(false);//verificando posse de bola

				  }
			}

    	   SsubExpress->out(1);//fecha no filho onde contem posicoes do corpo
    	   SsubExpress->skip(2);//pulando filho do no
    	   SsubExpress->out(1);//fechando no

    	   SsubExpress->skip(1);//pulando carga


    	   SsubExpress->in(2);//abrindo no para pegar posicoes da cabeça
    	   if(SsubExpress->take()=="SLT")
    	    	   {
    					transform = readTransformationMatrix(SsubExpress);
    					if (transform != NULL)
    					{
    						//agente3d.setHead(*(transform));//setamdp tranformacao  cabeça do agente

    						agente3dV[numOb].setHead(*transform);
    						if(agente3dV[numOb].getHeadAgentePosition().Z()< 0.2 && agente3dV[numOb].getStatus()==false){

    							agente3dV[numOb].setTimInici(time);
    							agente3dV[numOb].contQuedas();
    							agente3dV[numOb].setStatus(true);
    							graPosi= true;//variavel para ativar gravação das Matriz do corpo quando o agente cair
    						}else if(agente3dV[numOb].getStatus()==  true && agente3dV[numOb].getHeadAgentePosition().Z()> 0.2){
    							agente3dV[numOb].setStatus(false);
    							agente3dV[numOb].setTimCaido(time-agente3dV[numOb].getTemInici());
    						    graPosi=false;
    						}

    					  }
    				}



		   SsubExpress->out(1);//fecha no filho onde contem posicoes cabeça
		   SsubExpress->skip(1);//pula outro no filho
		   SsubExpress->out(1);//fecha no da mensagem cabeça
		   SsubExpress->skip(1);//pula carga


		   SsubExpress->in(2);//abrindo no para pegar posicoes braço superiorR
		   if(SsubExpress->take()=="SLT")
				   {
						transform = readTransformationMatrix(SsubExpress);
						if (transform != NULL)
						{

							agente3dV[numOb].setRmupperarm(*transform);

						  }
					}

		   SsubExpress->out(1);//fecha no filho onde contem posicoes do braço superiorR
		   SsubExpress->skip(1);//pula outro no filho
		   SsubExpress->out(1);//fecha no da mensagem braço superiorR

		   SsubExpress->skip(1);//pula carga


		   SsubExpress->in(2);//abrindo no para pegar posicoes Ante braçoR
		   if(SsubExpress->take()=="SLT")
				   {
						transform = readTransformationMatrix(SsubExpress);
						if (transform != NULL)
						{

							agente3dV[numOb].setRlowerarm(*transform);

						 }
					}
		   SsubExpress->out(1);//fecha no filho onde contem posicoes do Ante braçoR
		   SsubExpress->skip(1);//pula outro no filho
		   SsubExpress->out(1);//fecha no da mensagem Ante braçoR

		   SsubExpress->skip(1);//pula carga


		   SsubExpress->in(2);//abrindo no para pegar posicoes braço superiorL
		   if(SsubExpress->take()=="SLT")
				   {
						transform = readTransformationMatrix(SsubExpress);
						if (transform != NULL)
						{
							agente3dV[numOb].setLupperarm(*transform);

						  }
					}

		   SsubExpress->out(1);//fecha no filho onde contem posicoes do braço superiorL
		   SsubExpress->skip(1);//pula outro no filho
		   SsubExpress->out(1);//fecha no da mensagem braço superiorL

		   SsubExpress->skip(1);//pula carga

		   SsubExpress->in(2);//abrindo no para pegar posicoes Ante braçoL
				   if(SsubExpress->take()=="SLT")
						   {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{

									agente3dV[numOb].setLlowerarm(*transform);

								 }
					}
		   SsubExpress->out(1);//fecha no filho onde contem posicoes do Ante braço superiorR
		   SsubExpress->skip(1);//pula outro no filho
		   SsubExpress->out(1);//fecha no da mensagem braço superiorR

		   SsubExpress->skip(2);//pula duas cargas


		   SsubExpress->in(2);//abrindo no para pegar posicoes coixaR
		   if(SsubExpress->take()=="SLT")
				   {
						transform = readTransformationMatrix(SsubExpress);
						if (transform != NULL)
						{
							agente3dV[numOb].setRthigh(*transform);

						 }
			}
		   SsubExpress->out(1);//fecha no filho onde contem posicoes da coixaR
		   SsubExpress->skip(1);//pula outro no filho
		   SsubExpress->out(1);//fecha no da mensagem coixaR


		   SsubExpress->in(2);//abrindo no para pegar posicoes pernaR
		   if(SsubExpress->take()=="SLT")
				   {
						transform = readTransformationMatrix(SsubExpress);
						if (transform != NULL)
						{
							agente3dV[numOb].setRshank(*transform);

						 }
			}
		   SsubExpress->out(1);//fecha no filho onde contem posicoes da pernaR
		   SsubExpress->skip(1);//pula outro no filho
		   SsubExpress->out(1);//fecha no da mensagem braço superiorR

		   SsubExpress->skip(1);//pula  carga

		   SsubExpress->in(2);//abrindo no para pegar posicoes peR
		   if(SsubExpress->take()=="SLT")
				   {
						transform = readTransformationMatrix(SsubExpress);
						if (transform != NULL)
						{
							 agente3dV[numOb].setRfoot(*transform);

						 }
			}
		   SsubExpress->out(1);//fecha no filho onde contem posicoes da pernaR
		   SsubExpress->skip(1);//pula outro no filho
		   SsubExpress->out(1);//fecha no da mensagem braço superiorR

		   SsubExpress->skip(2);//pula duas cargas


		   SsubExpress->in(2);//abrindo no para pegar posicoes coixaL
		   if(SsubExpress->take()=="SLT")
				   {
						transform = readTransformationMatrix(SsubExpress);
						if (transform != NULL)
						{
							agente3dV[numOb].setLthigh(*transform);

						 }
			}
		   SsubExpress->out(1);//fecha no filho onde contem posicoes da coixaL
		   SsubExpress->skip(1);//pula outro no filho
		   SsubExpress->out(1);//fecha no da mensagem coixaL


		   SsubExpress->in(2);//abrindo no para pegar posicoes pernaL
		   if(SsubExpress->take()=="SLT")
				   {
						transform = readTransformationMatrix(SsubExpress);
						if (transform != NULL)
						{

							agente3dV[numOb].setLshank(*transform);

						 }
			}
		   SsubExpress->out(1);//fecha no filho onde contem posicoes da pernaL
		   SsubExpress->skip(1);//pula outro no filho
		   SsubExpress->out(1);//fecha no da mensagem pernaL

		   SsubExpress->skip(1);//pula  carga

		   SsubExpress->in(2);//abrindo no para pegar posicoes peL
		   if(SsubExpress->take()=="SLT")
				   {
						transform = readTransformationMatrix(SsubExpress);
						if (transform != NULL)
						{
							 agente3dV[numOb].setLfoot(*transform);

						 }
			}



		   SsubExpress->skipToEnd();//finaliza a string



		    agente3dV[numOb].setTime(time);//setando o tempo em que a mensagem foi envida
		   	estat.setDadosAgente(agente3dV[numOb],numOb);//passando os agentes do vetor do trainer para o vetor de estisticas.



	      }//fim do else que verificar se é a do vetor 0





}



void Trainer::atualizacaoAgentes(std::string subMsg,int numOb,double time){

	Matriz4d* transform=NULL;


	 ParserTrainer* SsubExpress = new ParserTrainer(subMsg);
	 std::string proxTonk;


       if(numOb==0){
	      SsubExpress->in(2);
	      if(SsubExpress->take()=="SLT"){

	    	  transform = readTransformationMatrix(SsubExpress);
	    	  			if (transform != NULL)
	    	  			{
	    	  			   agente3d.setBoll(*(transform));
	    	  			}
	      }
	      agente3d.setTime(time);

       }else{


          if(starInicial==true){
    	     agente3d.setStatus(false);//status do agente

          }

     	   SsubExpress->in(4);
    	   if(SsubExpress->take()=="SLT")
    	   {
					transform = readTransformationMatrix(SsubExpress);
					if (transform != NULL)
					{
						agente3d.setBody(*(transform));//setando a tranformação do corpo do agente

					  }
    	   	}

			SsubExpress->out(1);
			SsubExpress->in(3);

			 if(SsubExpress->take()=="nd")//pegando no para pagar número do agente
				  if(SsubExpress->take()=="StaticMesh"){ //reconhecendo  no do numero
					  SsubExpress->skip(3);
					  SsubExpress->in(1);
					  SsubExpress->take();
					  agente3d.setNumero(SsubExpress->take());//pega o numero do agente
					  //std::cout<<"Numero estanciado:"<<agente3d.getNumero()<<std::endl;
					  agente3d.setLado(SsubExpress->take());

				  }

    	    }

    	   SsubExpress->out(3);
    	   SsubExpress->skip(1);
    	   SsubExpress->out(1);
    	   SsubExpress->skip(1);

    	   SsubExpress->in(2);

    	   if(SsubExpress->take()=="SLT") {
    	      	   							transform = readTransformationMatrix(SsubExpress);
    	      	   							if (transform != NULL)
    	      	   							{
    	      	   								agente3d.setHead(*(transform));

    	      	   							}

    	   }

    	   SsubExpress->out(1);
    	   SsubExpress->skip(1);
    	   SsubExpress->out(1);
    	   SsubExpress->skip(1);

    	   SsubExpress->in(2);

    	   proxTonk=SsubExpress->take();

    	   if(proxTonk=="SLT") {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{
									agente3d.setRmupperarm(*(transform));

								}

    	    	   }

    	   SsubExpress->out(1);
    	   SsubExpress->skip(1);
    	   SsubExpress->out(1);
    	   SsubExpress->skip(1);

    	   SsubExpress->in(2);

            proxTonk=SsubExpress->take();
     	   if(proxTonk=="SLT") {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{
									agente3d.setRlowerarm(*(transform));

								}

        	    	   }

    	   SsubExpress->out(1);
    	   SsubExpress->skip(1);
    	   SsubExpress->out(1);
    	   SsubExpress->skip(1);

    	   SsubExpress->in(2);

            proxTonk=SsubExpress->take();
     	   if(proxTonk=="SLT") {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{
									agente3d.setLupperarm(*(transform));

								}

        	    	   }

    	   SsubExpress->out(1);
    	   SsubExpress->skip(1);
    	   SsubExpress->out(1);
    	   SsubExpress->skip(1);

    	   SsubExpress->in(2);

            proxTonk=SsubExpress->take();
     	   if(proxTonk=="SLT") {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{
									agente3d.setLlowerarm(*(transform));
								}

        	    	   }


		   SsubExpress->out(1);//fechando parte do no anterior
		   SsubExpress->skip(1);
		   SsubExpress->out(1);
		   SsubExpress->skip(2);//pulando duas cargas


		   SsubExpress->in(2);

			proxTonk=SsubExpress->take();
		   if(proxTonk=="SLT") {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{
									agente3d.setRthigh(*(transform));
								}

					   }

		   SsubExpress->out(1);//fechando parte do no anterior
		   SsubExpress->skip(1);
		   SsubExpress->out(1);

		   SsubExpress->in(2);

			proxTonk=SsubExpress->take();
		   if(proxTonk=="SLT") {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{
									agente3d.setRshank(*(transform));

								}

					   }


		   SsubExpress->out(1);//fechando parte do no anterior
		   SsubExpress->skip(1);
		   SsubExpress->out(1);
		   SsubExpress->skip(1);//pulando duas cargas


		   SsubExpress->in(2);

			proxTonk=SsubExpress->take();
		   if(proxTonk=="SLT") {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{
									 agente3d.setRfoot(*(transform));

								}

					   }

			SsubExpress->out(1);//fechando parte do no anterior
			SsubExpress->skip(1);
			SsubExpress->out(1);
			SsubExpress->skip(2);//pulando duas cargas


		   SsubExpress->in(2);

		   proxTonk=SsubExpress->take();
		   if(proxTonk=="SLT") {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{
									agente3d.setLthigh(*(transform));
								}

					   }


			 SsubExpress->out(1);//fechando parte do no anterior
			 SsubExpress->skip(1);
			 SsubExpress->out(1);

			 SsubExpress->in(2);

			 proxTonk=SsubExpress->take();

			 if(proxTonk=="SLT") {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{
									agente3d.setLshank(*(transform));

								}

					   }


			 SsubExpress->out(1);//fechando parte do no anterior
			 SsubExpress->skip(1);
			 SsubExpress->out(1);
			 SsubExpress->skip(1);//pulando duas cargas


			 SsubExpress->in(2);

			 proxTonk=SsubExpress->take();

			 if(proxTonk=="SLT") {
								transform = readTransformationMatrix(SsubExpress);
								if (transform != NULL)
								{
								   // std::cout<<"msg: "<<getMensageM<<std::endl;//imprimindo a mensagem
									//mAgentTransform =*(transform);
									agente3d.setLfoot(*(transform));
									//std::cout<< "\nPeL: "; agente3d.tostringPartAgent(agente3d.getLfootAgentePosition());

								}


			SsubExpress->skipToEnd();//finaliza a string


			agente3dV[numOb].setStatus(false);//mudando o status
			agente3dV[numOb]=agente3d;//inseri o agente com dados no vetor de agentes dentro do trainer

			estat.setAgentes(agente3d,numOb);


       }

}



void Trainer::mensagemParseTreiner(std::string content){//Metodo criado para enviar separa mensagem de cabecalho e mensagem do agente e bola

	int i;
	std::string token ;
	std::string no,noAux,plym ;
	std::string tipMensa;

	getMensageM =content;
	ParserTrainer* sexpression = new ParserTrainer(content);//recebe a mensagem total passado por paramento vinda do servidor

    /*Pegando primeira expressão*/
    std:: string exp_ver;

    exp_ver=sexpression->EXpress();//primeira chamada sempre traz o cabecalho (contem time, campo, traves entre outros)

    ParserTrainer* parseMensgem = new ParserTrainer(exp_ver);//recebe um no(cabeçalho) pai com seus filhos da mensagem para ser tratado na nova instancia

    parseMensgem->in(2);//abre duas aspa da mensagem passada(avança dois caracter)
    token =parseMensgem->take();//recebe o proximo token a dentro do no(cabeçalho) aberto

    if(token=="time"){//verificar se o primeiro token e o time se for a mensagem é parcial logo e uma mensagem de atualização

    	contaMensParcial++;

        mLastUpdateTime = convert_to<double>(parseMensgem->take());//pegar o valor do tempo para atualizar no agente

		parseMensgem->out(1); parseMensgem->in(1);

	    tipMensa=parseMensgem->take();

	    if(tipMensa =="score_right"){
			 sScoreRigth=parseMensgem->take();

	          parseMensgem->out(1);parseMensgem->in(1);
	          //plym=parseMensgem->take();
	          if(parseMensgem->take()=="play_mode"){
	             agente3dV[0].setPlayMode(parseMensgem->take());
	             sPlayMode=agente3dV[0].getPlayMode();
               }
	          parseMensgem->out(1);
	    }else if(tipMensa =="score_left"){  sScoreLeft=parseMensgem->take();

	          parseMensgem->out(1);parseMensgem->in(1);
		       if(parseMensgem->take()=="play_mode"){
			     agente3dV[0].setPlayMode(parseMensgem->take());
			     sPlayMode=agente3dV[0].getPlayMode();
		       }
		       parseMensgem->out(1);

	    }else if(tipMensa=="play_mode"){
    		 plym=parseMensgem->take();
    		 agente3dV[0].setPlayMode(plym);
    		 sPlayMode=agente3dV[0].getPlayMode();
    	 }

    	 tipMensa =sexpression->EXpress();//pega tipo da mensagem

    	 sexpression->in(1); sexpression->skip(35);//pula nosFIlhos do cabecalho

    	        for(i=0; i<sexpression->getExprLength();i++){

    	            noAux=sexpression->EXpress();
    	        	atualizarPosicoes(noAux,i, mLastUpdateTime);

    	        }
    	        contAgente = i;

    }else if(token=="FieldLength"){


    	parseMensgem->out(1);
    	parseMensgem->skip(15);

    	no=parseMensgem->EXpress();//pega o tempo dentro do cabeçalho

    	contMengCompl++;//variavel para contar quantas mensagens estao vindo completas
    	ParserTrainer* parseCabecalho = new ParserTrainer(no);//Instancia um objeto exp e colocar a mensagem de tempo para ser quebrada
    	parseCabecalho->in(1);

    	if(parseCabecalho->take()=="time"){//verificar se o toke tem que pegar o valor do tempo


				mLastUpdateTime = convert_to<double>(parseCabecalho->take()); //Atualizando  tempo

				tipMensa=sexpression->EXpress();

				sexpression->in(1);//entra em um parentese
				sexpression->skip(35);//pula 35 nos que abre e fecha no ()
				for(i=0; i<sexpression->getExprLength();i++){

					 noAux=sexpression->EXpress();

					 atualizacaoAgentes(noAux,i,mLastUpdateTime );

				}

    	    	 contAgente = i;

    	}
        starInicial=false;

    }else{

    	contaMensCortada++;

     }


}



int Trainer::posseDeBola(int ag){//funcao para verificar posse de bola

	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);
	Coordenada2d agente(agente3dV[ag].getBodyAgentePosition().X(),agente3dV[ag].getBodyAgentePosition().Y());
	Coordenada2d bola(agente3dV[ag].getBollAgente().Y(),agente3dV[ag].getBollAgente().Y());

	double distanciaAtual = Matematica::distancia(agente,bola);

	if((distanciaAtual <= (mDistanciaInicial/10)))
	{
		fim3 = false;
		mDistancia = distanciaAtual;
        return 1;
	}

	 return 0;

}



void Trainer::update(std::string content)
{
   // std::cout<<"msg: "<<content<<std::endl;

	//getMensageM = content;//gardando a mensagem inteira vinda do servidor
    ParserTrainer* sexpression = new ParserTrainer(content);
    Matriz4d* transform=NULL;
    std::mutex mutex;
    std::lock_guard<std::mutex> lock(mutex);

    ///std::cout<<sexpression->take()<<std::endl;

    if (sexpression->in(2)) {

        if (sexpression->take()=="time") {
            std::string timeString = sexpression->take();
            mLastUpdateTime = convert_to<double>(timeString);
            //std::cout<< "last update "<<mLastUpdateTime<<std::endl;
        }
        sexpression->out(2);
        if (sexpression->skip(1) && sexpression->in(1)
                && sexpression->skip(35) && sexpression->in(1)
                && sexpression->skip(1) && sexpression->in(1)
                && sexpression->skip(1)) {

        	  //std::cout<<sexpression->take()<<std::endl;

            transform = readTransformationMatrix(sexpression);
            if (transform != NULL)
            {
                mBallTransform = *transform;
               std::cout<< "tpos bolaa "<<mBallTransform.getPosition().toString()<<std::endl;
            }

            if (sexpression->out(2)) {
                while (true) {
                    if (sexpression->in(3)) {
                        if (sexpression->take()=="SLT") {
                            transform = readTransformationMatrix(sexpression);
                            if (transform != NULL)
                            {
                               // std::cout<<"msg: "<<getMensageM<<std::endl;//imprimindo a mensagem
                                mAgentTransform = *transform;
                                std::cout<< "tipos agentee: "<<mAgentTransform.getPosition().toString()<<std::endl;

                                break;
                            }
                        }
                    } else {
                        break;
                    }
                }

                	sexpression->out(2);

                    if(sexpression->in(2))
                    {
                        if(sexpression->take()=="SLT")
                        {
                            transform =readTransformationMatrix(sexpression);
                            if (transform != NULL)
                            {
                                mAgentTransformHead = *transform;
                                std::cout<< "tpos cabeca "<<mAgentTransformHead.getPosition().toString()<<std::endl;

                            }
                        }

                    }

            }//fim if para entrar na mensagem dos agentes
        }
        sexpression->skipToEnd();
    }
//    LOG(log_DEBUG)<<"fim update trainer";
}

Matriz4d*
Trainer::readTransformationMatrix(ParserTrainer* sexpression)
{
	double values[16];

	for (int i = 0; i < 16; i++) {
		std::string str = sexpression->take();
		//std::cout<<str; //imprimindo o vetor de transformacao
		if (str == "")
			return NULL;
		try {

			//std::cout<<str<<std::endl;//Imprimindos valores de posicao
			values[i] = convert_to<double>(str);
			//std::cout<<values[i]<<std::endl;
		} catch (BadParameter e) {return NULL;}
	}


	return new Matriz4d(values);
}

/*Retorna o true se o ponto1 está na área do ponto2*/
bool
Trainer::isWithinArea2(Coordenada3d point1,Coordenada3d point2)
{
	if( ((point1.X()-limitRaio) < point2.X()) && (point2.X() < point1.X()+limitRaio)
		&& (point2.Y() < point1.Y()+limitRaio) && ( point2.Y()  > point1.Y()-limitRaio)){
		return true;
	}
	else{
		return false;
	}
}

void
Trainer::imprimeEstatistica()
{
		std::mutex mutex;
		std::lock_guard<std::mutex> lock(mutex);

		mDistancia = mDistanciaInicial;
		mVelocidadeMedia = calcVelocity();
		mQuedasPorDistancia = getQuedasPorDistancia();
		mMediaTempoQuedas = calcMediaTempoQuedas();

		LOG(log_TRAINER) <<mVelocidadeMedia<<" "<<mQuedasPorDistancia<<" "<<mMediaTempoQuedas<<" "<<mChegou;
}

void
Trainer::aborteTeste()
{
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	//LOG(log_TRAINER) <<mMediaTempoQuedas<<" "<<mVelocidadeMedia<<" "<<mQuedasPorDistancia;

	setPlayMode("GameOver");
}

void
Trainer::setTempoMaximo(double time)
{
	mTempoMaximo = time;
}

void
Trainer::setIncQueda()
{
	mContQuedas++;
}

int
Trainer::getQtdQuedas()
{
	return mContQuedas;
}

void
Trainer::setDistancia(int distancia)
{
	mDistancia = distancia;
}

void
Trainer::setAnguloAgenteBola(double angulo)
{
	mAnguloAgenteBola = angulo;
}

void
Trainer::setTipo(int tipo)
{
	mTipo = tipo;
}

void
Trainer::setPosicaoXAgente(double posicao)
{
	mPosicaoXAgente = posicao;
}
void
Trainer::setSomaTempo(double tempo)//soma o tempo de queda do agente
{
	mSomaTempo += tempo;
}

double
Trainer::getSomaTempo()
{
	return mSomaTempo;
}
void
Trainer::setTempoInicial(double tempo)
{
	mTempo1 = tempo;
}


double
Trainer::getTempoInicial()
{
	return mTempo1;
}
void
Trainer::setTempoFinal(double tempo)
{
	mTempo2 = tempo;
}

double
Trainer::getTempoFinal()
{
	return mTempo2;
}

double
Trainer::calcMediaTempoQuedas()
{

	if(getQtdQuedas() == 0)
		return getLastUpdateTime();
	else
	if(getQtdQuedas() == 1)
	{
		if(mTempo1 < getLastUpdateTime() - mTempo1)
			return fabs(getLastUpdateTime() - mTempo1);
		else
			return mTempo1;
	}
	else
		return getSomaTempo()/ getQtdQuedas()-1;
}

double
Trainer::getQuedasPorDistancia()
{
	return getQtdQuedas()/mDistancia;
}

double
Trainer::calcVelocity(){

	return mDistancia/getLastUpdateTime();
}

/*
 * TODO: Calcular a posicao da bola com base no angulo em relação ao vetor frente do agente
 *  e da distância entre a bola e o agente.
 */
Coordenada3d
Trainer::posicaoBolaAngulo(double angulo)
{
	angulo = Matematica::grau2Rad(angulo);
	Coordenada3d agente = getAgentPosition();
	Coordenada3d bola = getBallTransform().getPosition();

	bola.X(agente.X() + (getAgentPosition().X()+mDistancia)*cos(angulo));
	bola.Y(agente.Y() + (getAgentPosition().X()+mDistancia)*sin(angulo));

	return bola;

}


/*
 * TODO:ExperimentoUm
 * É o primeiro experimento que pode ser usado. Ele calcula
 *   1 - VelocidadeMedia
 *   2 - Tempo médio entre quedas
 *   3 - Quantidade de quedas por distância
 */
void
Trainer::ExperimentoUm()
{

	if((getHeadAgentPosition().Z() < 0.4) && (mCaido == false))
	{
		mContQuedas++;
		mCaido = true;

		if(mContQuedas == 1)
			mTempo1 = getLastUpdateTime();

		if(mContQuedas > 1)
		{
			mTempo2 = getLastUpdateTime();
			mSomaTempo = mSomaTempo + (mTempo2 - mTempo1);
			mTempo1 = mTempo2;

		}

	}else if(getHeadAgentPosition().Z() > 0.4)
	{
		mCaido = false;
	}

}

/*
 * TODO: antes de chamar o experimento tres, chamar o ExperimentoUm
 */



void
Trainer::ExperimentoTres()
{

	Coordenada2d agente(getAgentPosition().X(),getAgentPosition().Y());
	Coordenada2d bola(getBallTransform().getPosition().X(),getBallTransform().getPosition().Y());
	double distanciaAtual = Matematica::distancia(agente,bola);

	//std::cerr<<distanciaAtual<<" "<<mDistanciaInicial<<std::endl;
	//Robô está a 50% da distância inicial da bola
	if((distanciaAtual <= (mDistanciaInicial/2)) && fim1)
	{
		fim1 = false;
		mDistancia = distanciaAtual;
		std::mutex mutex;
		std::lock_guard<std::mutex> lock(mutex);
		moveBall(posicaoBolaAngulo(-45));
	}
	//Robô está a 25% da distância inicial da bola
	if((distanciaAtual <= (mDistanciaInicial/4)) && fim2)
	{
		fim2 = false;
		mDistancia = distanciaAtual;
		std::mutex mutex;
		std::lock_guard<std::mutex> lock(mutex);
		moveBall(posicaoBolaAngulo(90));
	}
	//Robô está a 10% da distância inicial da bola
	if((distanciaAtual <= (mDistanciaInicial/10)) && fim3)
	{
		fim3 = false;
		mDistancia = distanciaAtual;
		std::mutex mutex;
		std::lock_guard<std::mutex> lock(mutex);
		moveBall(posicaoBolaAngulo(-45));
	}

}
